/**
 * @brief 控制机器人移动到指定位置和姿态（PID控制）
 * @param expect_x 目标X坐标（单位：米）
 * @param expect_y 目标Y坐标（单位：米）
 * @param expect_yaw 目标偏航角（单位：弧度）
 * @note 该函数使用PID控制器计算速度指令，并限制最大速度
 */
void Robot_Move_To_Target_Position(float expect_x, float expect_y, float expect_yaw)
{
    static uint8_t is_initialized = 0;  // 初始化标志位
    
    /* 第一次调用时设置目标位置 */
    if (is_initialized == 0)
    {  
        Robot.Robot_in_self.position_exp.x = expect_x;
        Robot.Robot_in_self.position_exp.y = expect_y;
        Robot.Robot_in_self.position_exp.yaw = expect_yaw;
        is_initialized = 1;
    }
    
    /* 计算位置误差 */
    float error_x = Robot.Robot_in_self.position_exp.x - Robot.Robot_in_self.position_now.x;
    float error_y = Robot.Robot_in_self.position_exp.y - Robot.Robot_in_self.position_now.y;
    float error_yaw = Robot.Robot_in_self.position_exp.yaw -Robot.Robot_in_self.position_now.yaw;
    
    /* PID计算期望速度 */
    Robot.Robot_in_world.velocity_exp_Unsure.Vx = PID_Calculate(&pid_error_x, error_x);
    Robot.Robot_in_world.velocity_exp_Unsure.Vy = PID_Calculate(&pid_error_y, error_y);
    Robot.Robot_in_world.velocity_exp_Unsure.Vw = PID_Calculate(&pid_error_yaw, error_yaw);
    
    /* 速度限幅处理 */
    Robot.Robot_in_world.velocity_exp.Vx = 
        Limit_Speed(Robot.Robot_in_world.velocity_exp.Vx, SPEED_LIMIT);
    Robot.Robot_in_world.velocity_exp.Vy = 
        Limit_Speed(Robot.Robot_in_world.velocity_exp.Vy, SPEED_LIMIT);
    Robot.Robot_in_world.velocity_exp.Vw = 
        Limit_Speed(Robot.Robot_in_world.velocity_exp.Vw, SPEED_LIMIT);
    
    /* 到达目标位置判断（误差小于0.1） */
    if ((fabs(error_x) < 0.1f) && 
        (fabs(error_y) < 0.1f) && 
        (fabs(error_yaw) < 0.1f))
    {
        Robot.Robot_in_world.velocity_exp.Vx = 0.0f;
        Robot.Robot_in_world.velocity_exp.Vy = 0.0f;
        Robot.Robot_in_world.velocity_exp.Vw = 0.0f;
        is_initialized = 0;  // 重置初始化标志
    }
}

/**
 * @brief 限制速度大小
 * @param speed 输入速度
 * @param limit 速度限制值
 * @return 限制后的速度
 */
float Limit_Speed(float speed, float limit)
{
    if (fabs(speed) > limit)
    {
        return limit * (speed > 0 ? 1.0f : -1.0f);
    }
    return speed;
}
